import time
from vuer import Vuer
from vuer.schemas import ImageBackground, Hands, MotionControllers
from multiprocessing import Array, Process, shared_memory
import numpy as np
import asyncio
import cv2
from multiprocessing import context
Value = context._default_context.Value
from teleop.utils.logger import get_logger
from teleop.utils.mat_tool import print_pose_info

# 获取日志记录器
logger = get_logger('TeleVision')

class TeleVision:
    def __init__(self, binocular, img_shape, img_shm_name, cert_file="./cert.pem", key_file="./key.pem", ngrok=False):
        self.binocular = binocular
        self.img_height = img_shape[0]
        if binocular:
            self.img_width  = img_shape[1] // 2
        else:
            self.img_width  = img_shape[1]

        logger.info(f"初始化TeleVision: binocular={binocular}, img_shape={img_shape}, img_width={self.img_width}")

        if ngrok:
            self.vuer = Vuer(host='0.0.0.0', queries=dict(grid=False), queue_len=3)
        else:
            self.vuer = Vuer(host='0.0.0.0', cert=cert_file, key=key_file, queries=dict(grid=False), queue_len=3)

        self.vuer.add_handler("CONTROLLER_MOVE")(self.on_controller_move)
        self.vuer.add_handler("CAMERA_MOVE")(self.on_cam_move)
     
        existing_shm = shared_memory.SharedMemory(name=img_shm_name)
        self.img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=existing_shm.buf)

        if binocular:
            # logger.info("使用双目模式，注册main_image_binocular处理器")
            self.vuer.spawn(start=False)(self.main_image_binocular)
        else:
            # logger.info("使用单目模式，注册main_image_monocular处理器")
            self.vuer.spawn(start=False)(self.main_image_monocular)

        self.left_hand_shared = Array('d', 16, lock=True)
        self.right_hand_shared = Array('d', 16, lock=True)
        self.left_landmarks_shared = Array('d', 75, lock=True)
        self.right_landmarks_shared = Array('d', 75, lock=True)
        
        self.head_matrix_shared = Array('d', 16, lock=True)
        self.aspect_shared = Value('d', 1.0, lock=True)
        
        # 添加共享内存来存储控制器状态
        # 右手柄状态：[squeeze(0/1), squeezeValue(0-1)]
        self.right_controller_shared = Array('d', 12, lock=True)
        # 左手柄状态：[squeeze(0/1), squeezeValue(0-1)]
        self.left_controller_shared = Array('d', 12, lock=True)
        
        # 初始化控制器状态
        self.left_controller_shared[0] = 0   # squeeze (0=False, 1=True)
        self.left_controller_shared[1] = 0   # squeezeValue (0-1)
        self.left_controller_shared[2] = 0   # trigger (0=False, 1=True)
        self.left_controller_shared[3] = 0   # triggerValue (0-1)
        self.left_controller_shared[4] = 0   # touchpad (0=False, 1=True)
        self.left_controller_shared[5] = 0   # touchpadX (0-1)
        self.left_controller_shared[6] = 0   # touchpadY (0-1)
        self.left_controller_shared[7] = 0   # thumbstick (0=False, 1=True)
        self.left_controller_shared[8] = 0   # thumbstickX (0-1)
        self.left_controller_shared[9] = 0   # thumbstickY (0-1)
        self.left_controller_shared[10] = 0  # aButton (0=False, 1=True)
        self.left_controller_shared[11] = 0  # bButton (0=False, 1=True)
        
        self.right_controller_shared[0] = 0  # squeeze (0=False, 1=True)
        self.right_controller_shared[1] = 0  # squeezeValue (0-1)
        self.right_controller_shared[2] = 0  # trigger (0=False, 1=True)
        self.right_controller_shared[3] = 0  # triggerValue (0-1)
        self.right_controller_shared[4] = 0  # touchpad (0=False, 1=True)
        self.right_controller_shared[5] = 0  # touchpadX (0-1)
        self.right_controller_shared[6] = 0  # touchpadY (0-1)
        self.right_controller_shared[7] = 0  # thumbstick (0=False, 1=True)
        self.right_controller_shared[8] = 0  # thumbstickX (0-1)
        self.right_controller_shared[9] = 0  # thumbstickY (0-1)
        self.right_controller_shared[10] = 0 # aButton (0=False, 1=True)
        self.right_controller_shared[11] = 0 # bButton (0=False, 1=True)

        self.process = Process(target=self.vuer_run)
        self.process.daemon = True
        self.process.start()

    
    def vuer_run(self):
        logger.info("开始运行vuer服务...")
        try:
            self.vuer.run()
        except Exception as e:
            logger.error(f"vuer运行出错: {str(e)}")
            import traceback
            logger.error(traceback.format_exc())

    async def on_cam_move(self, event, session, fps=60):
        # print("---------camera move", event.value['camera']['matrix'])
        try:
            self.head_matrix_shared[:] = event.value["camera"]["matrix"]
            self.aspect_shared.value = event.value['camera']['aspect']
        except:
            pass

    async def on_controller_move(self, event, session, fps=60):
    
        try:
            # 检查event.value是否存在并且是字典类型
            if hasattr(event, 'value') and isinstance(event.value, dict):
         
                
                # 根据实际收到的数据结构获取手柄数据
                # CONTROLLER_MOVE 事件包含手柄的位姿信息
                left = event.value.get("left")
                right = event.value.get("right")
                
                # 获取左右手柄的按键状态数据
                left_state = event.value.get("leftState", {})
                right_state = event.value.get("rightState", {})
                
                # 打印原始事件数据，便于调试
                # logger.debug(f"原始手柄事件数据: {event.value}")
                # logger.debug(f"左手柄原始状态: {left_state}")
                # logger.debug(f"右手柄原始状态: {right_state}")
                
                # 处理手柄位姿矩阵数据
                try:

                    left_squeeze = False
                    right_squeeze = False
                    
                    # 处理左手柄按键和摇杆数据
                    if left_state and isinstance(left_state, dict):
                        # 获取左手柄按键状态
                        left_trigger = left_state.get("trigger", False)  # 扳机键是否按下
                        left_squeeze = left_state.get("squeeze", False)  # 侧握键是否按下
                        left_touchpad = left_state.get("touchpad", False)  # 触摸板是否按下
                        left_thumbstick = left_state.get("thumbstick", False)  # 摇杆是否按下
                        left_a_button = left_state.get("aButton", False)  # A按钮是否按下
                        left_b_button = left_state.get("bButton", False)  # B按钮是否按下
                        
                        # 调试信息：打印左手柄按键状态
                        logger.debug(f"左手柄按键状态: trigger={left_trigger}, squeeze={left_squeeze}, touchpad={left_touchpad}, thumbstick={left_thumbstick}, aButton={left_a_button}, bButton={left_b_button}")
                        
                        # 获取左手柄模拟值
                        left_trigger_value = left_state.get("triggerValue", 0)  # 扳机键的模拟值（0-1）
                        left_squeeze_value = left_state.get("squeezeValue", 0)  # 侧握键的模拟值（0-1）
                        left_touchpad_value = left_state.get("touchpadValue", [0, 0])  # 触摸板的X、Y坐标值
                        left_thumbstick_value = left_state.get("thumbstickValue", [0, 0])  # 摇杆的X、Y坐标值
                        
                        # 可以在这里添加对左手柄按键和摇杆数据的处理逻辑
                        # 例如：根据按键状态执行特定操作
                        # if left_squeeze and left_squeeze_value > 0.5:
                        #     logger.info(f"左手柄侧握键被按下超过一半：{left_squeeze_value}")
                            # 执行相应操作...
                        
                        # if left_thumbstick_value[0] != 0 or left_thumbstick_value[1] != 0:
                        #     logger.info(f"左手柄摇杆位置：{left_thumbstick_value}")
                            # 根据摇杆位置执行操作...
                        
                        # 更新左手柄状态到共享内存
                        self.left_controller_shared[0] = 1.0 if left_squeeze else 0.0
                        self.left_controller_shared[1] = left_squeeze_value
                        
                        # 将完整的左手柄状态存储到共享内存中
                        # 添加所有按钮状态
                        self.left_controller_shared[2] = 1.0 if left_trigger else 0.0
                        self.left_controller_shared[3] = left_trigger_value
                        self.left_controller_shared[4] = 1.0 if left_touchpad else 0.0
                        self.left_controller_shared[5] = left_touchpad_value[0] if isinstance(left_touchpad_value, list) and len(left_touchpad_value) >= 2 else 0.0
                        self.left_controller_shared[6] = left_touchpad_value[1] if isinstance(left_touchpad_value, list) and len(left_touchpad_value) >= 2 else 0.0
                        self.left_controller_shared[7] = 1.0 if left_thumbstick else 0.0
                        self.left_controller_shared[8] = left_thumbstick_value[0] if isinstance(left_thumbstick_value, list) and len(left_thumbstick_value) >= 2 else 0.0
                        self.left_controller_shared[9] = left_thumbstick_value[1] if isinstance(left_thumbstick_value, list) and len(left_thumbstick_value) >= 2 else 0.0
                        self.left_controller_shared[10] = 1.0 if left_a_button else 0.0
                        self.left_controller_shared[11] = 1.0 if left_b_button else 0.0
                        
                        # 打印共享内存中的左手柄状态
                        logger.debug(f"共享内存中的左手柄状态: {self.left_controller_shared[:]}")
                    
                    # 处理右手柄按键和摇杆数据
                    if right_state and isinstance(right_state, dict):
                        # 获取右手柄按键状态
                        right_trigger = right_state.get("trigger", False)  # 扳机键是否按下
                        right_squeeze = right_state.get("squeeze", False)  # 侧握键是否按下
                        right_touchpad = right_state.get("touchpad", False)  # 触摸板是否按下
                        right_thumbstick = right_state.get("thumbstick", False)  # 摇杆是否按下
                        right_a_button = right_state.get("aButton", False)  # A按钮是否按下
                        right_b_button = right_state.get("bButton", False)  # B按钮是否按下
                        
                        # 调试信息：打印右手柄按键状态
                        logger.debug(f"右手柄按键状态: trigger={right_trigger}, squeeze={right_squeeze}, touchpad={right_touchpad}, thumbstick={right_thumbstick}, aButton={right_a_button}, bButton={right_b_button}")
                        
                        # 获取右手柄模拟值
                        right_trigger_value = right_state.get("triggerValue", 0)  # 扳机键的模拟值（0-1）
                        right_squeeze_value = right_state.get("squeezeValue", 0)  # 侧握键的模拟值（0-1）
                        right_touchpad_value = right_state.get("touchpadValue", [0, 0])  # 触摸板的X、Y坐标值
                        right_thumbstick_value = right_state.get("thumbstickValue", [0, 0])  # 摇杆的X、Y坐标值
                        
                        # 可以在这里添加对右手柄按键和摇杆数据的处理逻辑
                        # 例如：根据按键状态执行特定操作
                        # if right_squeeze and right_squeeze_value > 0.5:
                        #     logger.info(f"右手柄侧握键被按下超过一半：{right_squeeze_value}")
                            # 执行相应操作...
                        
                        # if right_thumbstick_value[0] != 0 or right_thumbstick_value[1] != 0:
                        #     logger.info(f"右手柄摇杆位置：{right_thumbstick_value}")
                            # 根据摇杆位置执行操作...
                        
                        # 更新右手柄状态到共享内存
                        self.right_controller_shared[0] = 1.0 if right_squeeze else 0.0
                        self.right_controller_shared[1] = right_squeeze_value
                        
                        # 将完整的右手柄状态存储到共享内存中
                        # 添加所有按钮状态
                        self.right_controller_shared[2] = 1.0 if right_trigger else 0.0
                        self.right_controller_shared[3] = right_trigger_value
                        self.right_controller_shared[4] = 1.0 if right_touchpad else 0.0
                        self.right_controller_shared[5] = right_touchpad_value[0] if isinstance(right_touchpad_value, list) and len(right_touchpad_value) >= 2 else 0.0
                        self.right_controller_shared[6] = right_touchpad_value[1] if isinstance(right_touchpad_value, list) and len(right_touchpad_value) >= 2 else 0.0
                        self.right_controller_shared[7] = 1.0 if right_thumbstick else 0.0
                        self.right_controller_shared[8] = right_thumbstick_value[0] if isinstance(right_thumbstick_value, list) and len(right_thumbstick_value) >= 2 else 0.0
                        self.right_controller_shared[9] = right_thumbstick_value[1] if isinstance(right_thumbstick_value, list) and len(right_thumbstick_value) >= 2 else 0.0
                        self.right_controller_shared[10] = 1.0 if right_a_button else 0.0
                        self.right_controller_shared[11] = 1.0 if right_b_button else 0.0
                        
                        # 打印共享内存中的右手柄状态
                        # logger.debug(f"共享内存中的右手柄状态: {self.right_controller_shared[:]}")
                        
                        # 打印调试信息
                        # logger.debug(f"更新右手柄状态到共享内存: squeeze={right_squeeze}, A按钮={right_a_button}, B按钮={right_b_button}")
                        # logger.debug(f"右手柄按键值: trigger={right_trigger_value}, squeeze={right_squeeze_value}")
                        # logger.debug(f"右手柄摇杆值: {right_thumbstick_value}")
                
                        # 检查左手柄B按钮是否正确
                        # if left_b_button:
                        #     logger.info(f"左手柄B按钮被按下: {left_b_button}, 原始值: {left_state.get('bButton', False)}")
                        
                        # 检查右手柄A和B按钮是否正确
                        # if right_a_button or right_b_button:
                        #     logger.info(f"右手柄按钮被按下: A={right_a_button}, B={right_b_button}")
                        #     logger.info(f"右手柄原始状态: {right_state}")
                            
                        # 检查摇杆数据
                        # if (isinstance(left_thumbstick_value, list) and len(left_thumbstick_value) >= 2 and 
                        #     (abs(left_thumbstick_value[0]) > 0.1 or abs(left_thumbstick_value[1]) > 0.1)):
                        #     logger.info(f"左手柄摇杆移动: {left_thumbstick_value}")
                            
                        # if (isinstance(right_thumbstick_value, list) and len(right_thumbstick_value) >= 2 and 
                        #     (abs(right_thumbstick_value[0]) > 0.1 or abs(right_thumbstick_value[1]) > 0.1)):
                        #     logger.info(f"右手柄摇杆移动: {right_thumbstick_value}")
                        
                    
                    if left and isinstance(left, list) and len(left) >= 16:
                        self.left_hand_shared[:] = left[:16]
                        # logger.info(f"------000 event left wrist:{print_pose_info(np.array(self.left_hand_shared[:]).reshape(4, 4))}")
                    
                    if right and isinstance(right, list) and len(right) >= 16:
                        self.right_hand_shared[:] = right[:16]
                        # logger.info(f"------000 event right wrist:{print_pose_info(np.array(self.right_hand_shared[:]).reshape(4, 4))}")
                except Exception as parse_error:
                    logger.error(f"---------解析手柄数据错误: {str(parse_error)}")
            else:
                logger.warning("---------事件值不是预期的字典类型或不存在")
        except Exception as e: 
            logger.error(f"---------controller move error: {str(e)}")
    
    async def main_image_binocular(self, session, fps=60):
        logger.info("---------binocular image sent")
        session.upsert @ MotionControllers(stream=True, key="motion-controller")
        session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False)
        while True:
            logger.info("---------binocular image sent")
            display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB)
            # aspect_ratio = self.img_width / self.img_height
            session.upsert(
                [
                    ImageBackground(
                        display_image[:, :self.img_width],
                        aspect=1.778,
                        height=1,
                        distanceToCamera=1,
                        # The underlying rendering engine supported a layer binary bitmask for both objects and the camera. 
                        # Below we set the two image planes, left and right, to layers=1 and layers=2. 
                        # Note that these two masks are associated with left eye’s camera and the right eye’s camera.
                        layers=1,
                        format="jpeg",
                        quality=50,
                        key="background-left",
                        interpolate=True,
                    ),
                    ImageBackground(
                        display_image[:, self.img_width:],
                        aspect=1.778,
                        height=1,
                        distanceToCamera=1,
                        layers=2,
                        format="jpeg",
                        quality=50,
                        key="background-right",
                        interpolate=True,
                    ),
                ],
                to="bgChildren",
            )
            # 'jpeg' encoding should give you about 30fps with a 16ms wait in-between.
            await asyncio.sleep(0.016 * 2)

    async def main_image_monocular(self, session, fps=60):
        logger.info("main_image_monocular方法被调用，开始初始化单目图像处理")
        try:
            # 将右侧的对象添加到左侧的 session
            session.upsert @ MotionControllers(stream=True, key="motion-controller")
            session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=True, showRight=True)
            logger.info("---------手部跟踪组件添加完成")
            
            frame_count = 0
            while True:
                try:
                    frame_count += 1
                    if frame_count % 100 == 0:  # 每100帧记录一次日志，避免日志过多
                        logger.info(f"---------monocular image sent (frame {frame_count})")
                    
                    # 检查图像数组是否有效
                    if self.img_array is None or self.img_array.size == 0:
                        logger.error("图像数组为空或无效")
                        await asyncio.sleep(0.1)
                        continue
                        
                    display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB)
                    # aspect_ratio = self.img_width / self.img_height
                    
                    # 记录图像信息
                    if frame_count % 100 == 0:
                        logger.info(f"图像形状: {display_image.shape}, 数据类型: {display_image.dtype}")
                    
                    session.upsert(
                        [
                            ImageBackground(
                                display_image,
                                aspect=1.778,
                                height=1,
                                distanceToCamera=1,
                                format="jpeg",
                                quality=50,
                                key="background-mono",
                                interpolate=True,
                            ),
                        ],
                        to="bgChildren",
                    )
                    await asyncio.sleep(0.016)
                except Exception as e:
                    logger.error(f"单目图像处理循环出错: {str(e)}")
                    await asyncio.sleep(0.1)
        except Exception as e:
            logger.error(f"main_image_monocular方法出错: {str(e)}")
            import traceback
            logger.error(traceback.format_exc())
            

    @property
    def left_hand(self):
        return np.array(self.left_hand_shared[:]).reshape(4, 4, order="F")
        
    
    @property
    def right_hand(self):
        return np.array(self.right_hand_shared[:]).reshape(4, 4, order="F")
    
    def left_landmarks(self):
        return np.array(self.left_landmarks_shared[:]).reshape(25, 3)
    
    def right_landmarks(self):
        return np.array(self.right_landmarks_shared[:]).reshape(25, 3)
    
    def head_matrix(self):
        return np.array(self.head_matrix_shared[:]).reshape(4, 4, order="F")
    
    def aspect(self):
        return self.aspect_shared.value
    
    def right_controller(self):
        # 返回右手柄状态 [squeeze(0/1), squeezeValue(0-1)]
        return np.array(self.right_controller_shared[:])
    
    def left_controller(self):
        # 返回左手柄状态 [squeeze(0/1), squeezeValue(0-1)]
        return np.array(self.left_controller_shared[:])

if __name__ == '__main__':
    import os 
    import sys
    current_dir = os.path.dirname(os.path.abspath(__file__))
    parent_dir = os.path.dirname(current_dir)
    sys.path.append(parent_dir)
    import threading
    from image_server.image_client import ImageClient

    # image
    img_shape = (480, 640 * 2, 3)
    img_shm = shared_memory.SharedMemory(create=True, size=np.prod(img_shape) * np.uint8().itemsize)
    img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=img_shm.buf)
    img_client = ImageClient(tv_img_shape = img_shape, tv_img_shm_name = img_shm.name)
    image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True)
    image_receive_thread.start()

    # television
    tv = TeleVision(True, img_shape, img_shm.name)
    print("vuer unit test program running...")
    print("you can press ^C to interrupt program.")
    while True:
        time.sleep(0.03)